/*
 * sysinit.c
 */
#include "sysinit.h"
#include "mcg.h"
#include "uart.h"
#include "nvic.h"
#include "port.h"
#include "Sw_Timers.h"
#include "evhdl_iterface.h"
#include "i2c_interface.h"
#include "GDD_Interface.h"

CPU_tdSinit CPU_tData;

static void SINI__CPUinit(void);
static void SINI__vRegistreInterrupts(void);
static void SINI__InitDrivers(void);
static void SINI__InitServices(void);
static void SINI__InitApplication(void);
static void SysTick_Config(uint32_t ticks);



void SINI_vSystemInit(void)
{
	SINI__CPUinit();
	SINI__InitServices();
	SINI__InitDrivers();
	SINI__vRegistreInterrupts();		
	SINI__InitApplication();
	/* Enable System Interrupts */
	EnableInterrupts;
   SysTick_Config(CPU_tData.core_clk_khz);
}

#ifdef MCU_MKL25Z4

static void SINI__CPUinit(void)
{
	 /* Enable all of the port clocks. These have to be enabled to configure
	 * pin muxing options, so most code will need all of these on anyway.
	 */
	SIM_SCGC5 |= (SIM_SCGC5_PORTA_MASK
			  | SIM_SCGC5_PORTB_MASK
			  | SIM_SCGC5_PORTC_MASK
			  | SIM_SCGC5_PORTD_MASK
			  | SIM_SCGC5_PORTE_MASK );
	
	/* Ramp up the system clock */
	/* Set the system dividers */
	/* NOTE: The PLL init will not configure the system clock dividers,
	* so they must be configured appropriately before calling the PLL
	* init function to ensure that clocks remain in valid ranges.
	*/  
	SIM_CLKDIV1 = ( 0
					| SIM_CLKDIV1_OUTDIV1(0)
					| SIM_CLKDIV1_OUTDIV4(1) );

#if defined(NO_PLL_INIT)
        // If PLL initialization is not desired, set FLL to 48 MHz clock in default FEI mode
        //MCG_C4 |= (MCG_C4_DRST_DRS(1) | MCG_C4_DMX32_MASK);
		CPU_tData.mcg_clk_hz = 21000000; //FEI mode
        
        SIM_SOPT2 &= ~SIM_SOPT2_PLLFLLSEL_MASK; // clear PLLFLLSEL to select the FLL for this clock source
        
        CPU_tData.uart0_clk_khz = (mcg_clk_hz / 1000); // the uart0 clock frequency will equal the FLL frequency
#else
	
	/* Initialize PLL */
	/* PLL will be the source for MCG CLKOUT so the core, system, and flash clocks are derived from it */ 
    CPU_tData.mcg_clk_hz = pll_init(CLK0_FREQ_HZ,  /* CLKIN0 frequency */
							 LOW_POWER,     /* Set the oscillator for low power mode */
							 CLK0_TYPE,     /* Crystal or canned oscillator clock input */
							 PLL0_PRDIV,    /* PLL predivider value */
							 PLL0_VDIV,     /* PLL multiplier */
							 MCGOUT);       /* Use the output from this PLL as the MCGOUT */
	
	/*
	 * Use the value obtained from the pll_init function to define variables
	 * for the core clock in kHz and also the peripheral clock. These
	 * variables can be used by other functions that need awareness of the
	 * system frequency.
	 */
    CPU_tData.mcg_clk_khz = CPU_tData.mcg_clk_hz / 1000;
    CPU_tData.core_clk_khz = CPU_tData.mcg_clk_khz / (((SIM_CLKDIV1 & SIM_CLKDIV1_OUTDIV1_MASK) >> 28)+ 1);
    CPU_tData.periph_clk_khz = CPU_tData.core_clk_khz / (((SIM_CLKDIV1 & SIM_CLKDIV1_OUTDIV4_MASK) >> 16)+ 1);
    CPU_tData.mcgpllclk_khz = CPU_tData.mcg_clk_khz / 2;
	
	SIM_SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK; // set PLLFLLSEL to select the PLL for this clock source
#endif

	SIM_SOPT2 |= SIM_SOPT2_UART0SRC(1); // select the PLLFLLCLK as UART0 clock source
	CPU_tData.uart0_clk_khz = ((CPU_tData.mcg_clk_hz / 2) / 1000); // UART0 clock frequency will equal half the PLL frequency
	uart0_init (UART0_BASE_PTR, CPU_tData.uart0_clk_khz, TERMINAL_BAUD);  
}

#else

static void SINI__CPUinit(void)
{
	/* Enable all of the port clocks. These have to be enabled to configure
	 * pin muxing options, so most code will need all of these on anyway.
	 */
	SIM_SCGC5 |= (SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK);
	
	// releases hold with ACKISO:  Only has an effect if recovering from VLLS1, VLLS2, or VLLS3
	// if ACKISO is set you must clear ackiso before calling pll_init 
	//    or pll init hangs waiting for OSC to initialize
	// if osc enabled in low power modes - enable it first before ack
	// if I/O needs to be maintained without glitches enable outputs and modules first before ack.
	if (PMC_REGSC &  PMC_REGSC_ACKISO_MASK)
	PMC_REGSC |= PMC_REGSC_ACKISO_MASK;

#if defined(NO_PLL_INIT)
        // If PLL initialization is not desired, set FLL to 48 MHz clock in default FEI mode
        //MCG_C4 |= (MCG_C4_DRST_DRS(1) | MCG_C4_DMX32_MASK);
		CPU_tData.mcg_clk_hz = 21000000; //FEI mode
                    
#else	
       /* Ramp up the system clock */
	   /* Set the system dividers */
	  
		SIM_CLKDIV1 = ( 0
						| SIM_CLKDIV1_OUTDIV1(0)
						| SIM_CLKDIV1_OUTDIV4(1) 
					  );
	  mcg_clk_hz =  fei_fee(CLK0_FREQ_HZ,HIGH_GAIN,CLK0_TYPE);
#endif	
	/*
	 * Use the value obtained from the pll_init function to define variables
	 * for the core clock in kHz and also the peripheral clock. These
	 * variables can be used by other functions that need awareness of the
	 * system frequency.
	 */
    CPU_tData.mcg_clk_khz = CPU_tData.mcg_clk_hz / 1000;
    CPU_tData.core_clk_khz = CPU_tData.mcg_clk_khz / (((SIM_CLKDIV1 & SIM_CLKDIV1_OUTDIV1_MASK) >> 28)+ 1);
    CPU_tData.periph_clk_khz = CPU_tData.core_clk_khz / (((SIM_CLKDIV1 & SIM_CLKDIV1_OUTDIV4_MASK) >> 16)+ 1);    
	
		
	SIM_SOPT2 |= SIM_SOPT2_UART0SRC(1); // select the MCGFLLCLK as UART0 clock source
	CPU_tData.uart0_clk_khz = (CPU_tData.mcg_clk_hz / 1000); // UART0 clock frequency will equal half the PLL frequency
	uart0_init (UART0_BASE_PTR, CPU_tData.uart0_clk_khz, TERMINAL_BAUD);
	

}

#endif


static void SINI__vRegistreInterrupts(void){
	
	/*Enable Interrupt for UART0*/	
	enable_irq(INT_UART0 -16);
	/*Enable Interrupt for TPM0*/
	enable_irq(INT_TPM0 -16);	
	/*Enable Interrupt for I2C0*/
	enable_irq(INT_I2C0 -16);	
	/*Enable Interrupt for IRQ Port A*/
	enable_irq(INT_PORTA -16);  
	/*Enable Interrupt for SysTick timer*/
	enable_irq(INT_SysTick -16); 
	
	
	/*Set interrupts priority*/
	set_irq_priority((INT_UART0 -16), 2);	
	set_irq_priority((INT_TPM0 -16), 2);
	set_irq_priority((INT_I2C0 -16), 2);
	set_irq_priority((INT_PORTA -16), 2);
	set_irq_priority((INT_SysTick - 16), 2); 
}

static void SINI__InitDrivers(void)
{
	PORT_vPinMuxInit();
	
	/* TPM module initialisation */
	//(void)TPM_Init(TPM__nTPM0,TPM_enMCGFLLCLK,CPU_tData.mcgpllclk_khz,0x01);
	TPM_init_PWM(TPM__nTPM1, TPM_PLLFLL, TPM_MODULE, TPM_CLK, PS_128, EDGE_PWM);
	TPM_CH_init(TPM__nTPM1, 0, TPM_PWM_H);
	set_TPM_CnV(TPM__nTPM1, 0, DUTY_MAX);
	
	TPM_CH_init(TPM__nTPM1, 1, TPM_PWM_H);
	set_TPM_CnV(TPM__nTPM1, 1, DUTY_MAX);
	
	TPM_init_PWM(TPM__nTPM0, TPM_PLLFLL, TPM_MODULE, TPM_CLK, PS_128, EDGE_PWM);
	TPM_CH_init(TPM__nTPM0, 1, TPM_PWM_H);
	set_TPM_CnV(TPM__nTPM0, 1, DUTY_MAX);
	   /*Todo: Move this functions to PWM module*/
	
	
	/* PORT module initialisation */
	PORT_vDataDirectionInit();
	PORT_vOutputInit();
	
	/* I2C module initialisation */
	I2C_vInitHW(I2C__nChannel0, I2C_MASTER_ADDR, 600000);
	I2C_vSini();
}

static void SINI__InitServices(void)
{
	/* Event Handler module Init*/
	EVHD_vSInit();
	/* SW timer module initialization */
	SwTimer_Init();	
}

static void SINI__InitApplication(void)
{
	/*App initialization */
   GDD_vInit();
}


/* ##################################    SysTick function  ############################################ */
/** \ingroup  CMSIS_Core_FunctionInterface
    \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
    \brief      Functions that configure the System.
  @{
 */


/** \brief  System Tick Configuration

    The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
    Counter is in free running mode to generate periodic interrupts.

    \param [in]  ticks  Number of ticks between two interrupts.

    \return          0  Function succeeded.
    \return          1  Function failed.

    \note     When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
    function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
    must contain a vendor-specific implementation of this function.

 */
static void SysTick_Config(uint32_t u32ticks)
{
   /* set reload register */
   SysTick_RVR_REG(SysTick_BASE_PTR) = u32ticks - 1;

   /* Load the SysTick Counter Value */
   SysTick_CVR_REG(SysTick_BASE_PTR) = 0;

   /* Enable SysTick IRQ and SysTick Timer */
   SysTick_CSR_REG(SysTick_BASE_PTR) = SysTick_CSR_CLKSOURCE_MASK 
         | SysTick_CSR_TICKINT_MASK 
         | SysTick_CSR_ENABLE_MASK;

}


void SysTick_Handler(void)
{
   SysTick__ISR();
}












